#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stdbool.h>

#include "lwip/inet.h"
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>

#include "LoRaPhy_interface.h"
#include "gw-device.h"

#include "cJSON.h"

#include "base64.h"
#include "jitqueue.h"
#include "LoRaPhy_interface.h"
#include "loragw_gpio.h"

#include "lora_pkt_fwd.h"
#include "esp_lora.h"

#include "pthread.h"


#define PKT_PUSH_DATA   0
#define PKT_PUSH_ACK    1
#define PKT_PULL_DATA   2
#define PKT_PULL_RESP   3
#define PKT_PULL_ACK    4
#define PKT_TX_ACK      5

/* -------------------------------------------------------------------------- */
/* --- PRIVATE MACROS ------------------------------------------------------- */

#define ARRAY_SIZE(a)   (sizeof(a) / sizeof((a)[0]))
#define STRINGIFY(x)    #x
#define STR(x)          STRINGIFY(x)


#define STATUS_SIZE     300

#define MSG CON_PRINTF

/* -------------------------------------------------------------------------- */
/* --- PRIVATE CONSTANTS ---------------------------------------------------- */

/* Just In Time TX scheduling */
static struct jit_queue_s jit_queue;

/* network sockets */
static int sock_up; /* socket for upstream traffic */
static int sock_down; /* socket for downstream traffic */

static SemaphoreHandle_t jit_protect_semaphore = NULL;


uint32_t net_mac_h = 0;
uint32_t net_mac_l = 0;

static struct timeval push_timeout_half = {0, (PUSH_TIMEOUT_MS * 500)}; /* cut in half, critical for throughput */
static struct timeval pull_timeout = {0, (PULL_TIMEOUT_MS * 1000)}; /* non critical for throughput */

/* measurements to establish statistics */
static pthread_mutex_t mx_meas_up = PTHREAD_MUTEX_INITIALIZER; /* control access to the upstream measurements */
static uint32_t meas_nb_rx_rcv = 0; /* count packets received */
static uint32_t meas_nb_rx_ok = 0; /* count packets received with PAYLOAD CRC OK */
static uint32_t meas_nb_rx_bad = 0; /* count packets received with PAYLOAD CRC ERROR */
static uint32_t meas_nb_rx_nocrc = 0; /* count packets received with NO PAYLOAD CRC */
static uint32_t meas_up_pkt_fwd = 0; /* number of radio packet forwarded to the server */
static uint32_t meas_up_network_byte = 0; /* sum of UDP bytes sent for upstream traffic */
static uint32_t meas_up_payload_byte = 0; /* sum of radio payload bytes sent for upstream traffic */
static uint32_t meas_up_dgram_sent = 0; /* number of datagrams sent for upstream traffic */
static uint32_t meas_up_ack_rcv = 0; /* number of datagrams acknowledged for upstream traffic */

static pthread_mutex_t mx_meas_dw = PTHREAD_MUTEX_INITIALIZER; /* control access to the downstream measurements */
static uint32_t meas_dw_pull_sent = 0; /* number of PULL requests sent for downstream traffic */
static uint32_t meas_dw_ack_rcv = 0; /* number of PULL requests acknowledged for downstream traffic */
static uint32_t meas_dw_dgram_rcv = 0; /* count PULL response packets received for downstream traffic */
static uint32_t meas_dw_network_byte = 0; /* sum of UDP bytes sent for upstream traffic */
static uint32_t meas_dw_payload_byte = 0; /* sum of radio payload bytes sent for upstream traffic */
static uint32_t meas_nb_tx_ok = 0; /* count packets emitted successfully */
static uint32_t meas_nb_tx_fail = 0; /* count packets were TX failed for other reasons */
static uint32_t meas_nb_tx_requested = 0; /* count TX request from server (downlinks) */
static uint32_t meas_nb_tx_rejected_collision_packet = 0; /* count packets were TX request were rejected due to collision with another packet already programmed */
static uint32_t meas_nb_tx_rejected_collision_beacon = 0; /* count packets were TX request were rejected due to collision with a beacon already programmed */
static uint32_t meas_nb_tx_rejected_too_late = 0; /* count packets were TX request were rejected because it is too late to program it */
static uint32_t meas_nb_tx_rejected_too_early = 0; /* count packets were TX request were rejected because timestamp is too much in advance */
static uint32_t meas_nb_beacon_queued = 0; /* count beacon inserted in jit queue */
static uint32_t meas_nb_beacon_sent = 0; /* count beacon actually sent to concentrator */
static uint32_t meas_nb_beacon_rejected = 0; /* count beacon rejected for queuing */

static pthread_mutex_t mx_stat_rep = PTHREAD_MUTEX_INITIALIZER; /* control access to the status report */
static bool report_ready = false; /* true when there is a new report to send to the server */
static char status_report[STATUS_SIZE]; /* status report as a JSON object */

// static bool exit_signal = false;//退出lora packet-forward的线程

void thread_up(void* args);
void thread_down(void* args);
void thread_jit(void* args);
void thread_stat(void* args);

static int udp_write(int fd,uint8_t* buff,int len,char* func_name);

void setGWNetMac(uint32_t mac_h,uint32_t mac_l)
{
    /* process some of the configuration variables */
    net_mac_h = htonl((uint32_t)(mac_h));
    net_mac_l = htonl((uint32_t)(mac_l));

}

void pkt_fwd_runningException_handler()
{
    //当pkt_forward运行异常时，
    //1. 结束packet-forward，重新连接
    //2. 重启设备，packet-forward服务即重新启动  <--> 此处选择这种办法
    
    ESP_LOGE("pkt-fwd","runningException occured");
    ESP_LOGE("pkt-fwd","Error NS Service ");
    ESP_LOGE("pkt-fwd","device will restart 60s later");
    vTaskDelay(60000);
    printf("system restart......\n");
    esp_restart();

}

int txpkjson_to_rfPara(RF_Parameters_t* tx_para,uint8_t* txpk_json)
{
    int ret = 0;
    cJSON* root = cJSON_Parse((char*)txpk_json);
    cJSON *txpk,*tmst,*imme,*freq,*rfch,*powe,*modu,*datr,*codr,*ipol,*size,*data;
    txpk = cJSON_GetObjectItem(root,"txpk");
    if(txpk == NULL)
        return -1;

    tmst = cJSON_GetObjectItem(txpk,"tmst");
    imme = cJSON_GetObjectItem(txpk,"imme");
    freq = cJSON_GetObjectItem(txpk,"freq");
    rfch = cJSON_GetObjectItem(txpk,"rfch");
    powe = cJSON_GetObjectItem(txpk,"powe");
    modu = cJSON_GetObjectItem(txpk,"modu");
    datr = cJSON_GetObjectItem(txpk,"datr");
    codr = cJSON_GetObjectItem(txpk,"codr");
    ipol = cJSON_GetObjectItem(txpk,"ipol");
    size = cJSON_GetObjectItem(txpk,"size");
    data = cJSON_GetObjectItem(txpk,"data");


    if((imme==NULL) || (freq==NULL) || (rfch==NULL) ||
       (powe==NULL) || (modu==NULL) || (datr==NULL) ||
       (codr==NULL) || (ipol==NULL) || (size==NULL) ||(data==NULL))
    {
        ESP_LOGW("txpk format error","json error");
        ret = -1;
        goto out;
    }


    if(!memcmp(modu->valuestring,"LORA",strlen("LORA")))//modu == LORA
    {
        tx_para->modulation = MODEM_LORA;
    }
    else 
    {
        tx_para->modulation = MODEM_FSK;
        ret = -1;
        goto out;
    }

    tx_para->crc = false;
    tx_para->invert_pol = true;
    tx_para->preamble = 8;
    tx_para->rf_power = 20;
    tx_para->SYNC_word = 0x34;

    tx_para->tx_info.imme = imme->valueint;
    tx_para->freq_hz = freq->valuedouble*1000000;
    tx_para->rf_power = powe->valueint;


    if(tmst != NULL)
        tx_para->tx_info.count_ms = tmst->valueint/1000;
    else
        tx_para->tx_info.count_ms = 0;

    int sf,bw;
    sscanf(datr->valuestring,"SF%dBW%d",&sf,&bw);

    switch(sf)
    {
        case 6:
            tx_para->datarate = LoRa_SF6;    
            break;

        case 7:
            tx_para->datarate = LoRa_SF7;
            break;

        case 8:
            tx_para->datarate = LoRa_SF8;
            break;
        
        case 9:    
            tx_para->datarate = LoRa_SF9;
            break;

        case 10:
            tx_para->datarate = LoRa_SF10;
            break;
        
        case 11:
            tx_para->datarate = LoRa_SF11;
            break;
        
        case 12:
            tx_para->datarate = LoRa_SF12;
            break;

        default:
            ret = -1;
            goto out;
    }

    switch(bw)
    {
        case 125:
            tx_para->bandwidth = BANDWIDTH_125KHZ;
            break;
        
        case 250:
            tx_para->bandwidth = BANDWIDTH_250KHZ;        
            break;
        
        case 500:
            tx_para->bandwidth = BANDWIDTH_500KHZ;        
            break;

        default:
            ret = -1;
            goto out;
    }

    if(!memcmp(codr->valuestring,"4/5",strlen("4/5")))
    {
        tx_para->coderate = LORA_CR_4_5;
    }
    else if(!memcmp(codr->valuestring,"4/6",strlen("4/6")))
    {
        tx_para->coderate = LORA_CR_4_6;        
    }
    else if(!memcmp(codr->valuestring,"4/7",strlen("4/7")))
    {
        tx_para->coderate = LORA_CR_4_7;                
    }
    else if(!memcmp(codr->valuestring,"4/8",strlen("4/8")))
    {
        tx_para->coderate = LORA_CR_4_8;                
    }
    else 
    {
        ret = -1;
        goto out;
    }

    int j = b64_to_bin(data->valuestring,strlen(data->valuestring),tx_para->payload,255);
    if(j > 0)
    {
        tx_para->size = j;
    }
    else 
    {
        ret = -1;
        goto out;
    }

    if(tx_para->size != size->valueint)
    {
        ret = -1;
        goto out;
    }
    else 
    {
        ret = 0;
    }

    // cJSON *txpk,*tmst,*imme,*freq,*rfch,*powe,*modu,*datr,*codr,*ipol,*size,*data;

    // free(txpk);
    // free(tmst);
    // free(imme);
    // free(freq);
    // free(rfch);
    // free(powe);
    // free(modu);
    // free(datr);
    // free(codr);
    // free(ipol);
    // free(size);
    // free(data);

out:
    cJSON_Delete(root);


    return ret;
}

void get_PULL_DATA_frame(uint8_t* buff_req)
{
     /* pre-fill the pull request buffer with fixed fields */
    buff_req[0] = PROTOCOL_VERSION;
    buff_req[3] = PKT_PULL_DATA;
    
    *(uint32_t *)(buff_req + 4) = net_mac_h;
    *(uint32_t *)(buff_req + 8) = net_mac_l;   

    /* generate random token for request */
    buff_req[1] = (uint8_t)rand();
    buff_req[2] = (uint8_t)rand();
}

int get_tx_ack(uint8_t token_h, uint8_t token_l, enum jit_error_e error,uint8_t* buf_dest) 
{
    uint8_t* buff_ack = buf_dest ; 
    int buff_index;

    /* Prepare downlink feedback to be sent to server */
    buff_ack[0] = PROTOCOL_VERSION;
    buff_ack[1] = token_h;
    buff_ack[2] = token_l;
    buff_ack[3] = PKT_TX_ACK;
    *(uint32_t *)(buff_ack + 4) = net_mac_h;
    *(uint32_t *)(buff_ack + 8) = net_mac_l;
    buff_index = 12; /* 12-byte header */

    /* Put no JSON string if there is nothing to report */
    if (error != JIT_ERROR_OK) {
        /* start of JSON structure */
        memcpy((void *)(buff_ack + buff_index), (void *)"{\"txpk_ack\":{", 13);
        buff_index += 13;
        /* set downlink error status in JSON structure */
        memcpy((void *)(buff_ack + buff_index), (void *)"\"error\":", 8);
        buff_index += 8;
        switch (error) {
            case JIT_ERROR_FULL:
            case JIT_ERROR_COLLISION_PACKET:
                memcpy((void *)(buff_ack + buff_index), (void *)"\"COLLISION_PACKET\"", 18);
                buff_index += 18;
                break;
            case JIT_ERROR_TOO_LATE:
                memcpy((void *)(buff_ack + buff_index), (void *)"\"TOO_LATE\"", 10);
                buff_index += 10;
                break;
            case JIT_ERROR_TOO_EARLY:
                memcpy((void *)(buff_ack + buff_index), (void *)"\"TOO_EARLY\"", 11);
                buff_index += 11;
                break;
            case JIT_ERROR_COLLISION_BEACON:
                memcpy((void *)(buff_ack + buff_index), (void *)"\"COLLISION_BEACON\"", 18);
                buff_index += 18;
                break;
            case JIT_ERROR_TX_FREQ:
                memcpy((void *)(buff_ack + buff_index), (void *)"\"TX_FREQ\"", 9);
                buff_index += 9;
                break;
            case JIT_ERROR_TX_POWER:
                memcpy((void *)(buff_ack + buff_index), (void *)"\"TX_POWER\"", 10);
                buff_index += 10;
                break;
            case JIT_ERROR_GPS_UNLOCKED:
                memcpy((void *)(buff_ack + buff_index), (void *)"\"GPS_UNLOCKED\"", 14);
                buff_index += 14;
                break;
            default:
                memcpy((void *)(buff_ack + buff_index), (void *)"\"UNKNOWN\"", 9);
                buff_index += 9;
                break;
        }
        /* end of JSON structure */
        memcpy((void *)(buff_ack + buff_index), (void *)"}}", 2);
        buff_index += 2;
    }

    buff_ack[buff_index] = 0; /* add string terminator, for safety */

    /* send datagram to server */
    return buff_index;
}

int generate_pkt_fwd_fd(char* server_ip,char* server_port)
{
    /* network socket creation */
    struct addrinfo hints;
    struct addrinfo *result; /* store result of getaddrinfo */
    struct addrinfo *q; /* pointer to move into *result data */

    /* prepare hints to open network sockets */
    memset(&hints, 0, sizeof hints);
    hints.ai_family = AF_INET; /* WA: Forcing IPv4 as AF_UNSPEC makes connection on localhost to fail */
    hints.ai_socktype = SOCK_DGRAM;

    char serv_addr[64] = {0}; /* address of the server (host name or IPv4/IPv6) */
    char serv_port[8] = {0}; /* server port for upstream traffic */
    int ret = 0;

    // int temp_sock_up = 0;
    // int temp_sock_down = 0;

    memcpy(serv_addr,server_ip,strlen(server_ip));
    memcpy(serv_port,server_port,strlen(server_port));

    ESP_LOGI("CONNECT","server:%s,port:%s\n",server_ip,server_port);

    /* look for server address w/ upstream port */
    ret = getaddrinfo(serv_addr, serv_port, &hints, &result);
    if (ret != 0) {
        CON_PRINTF("ERROR: [up] getaddrinfo on address %s (PORT %s) returned %s\n", serv_addr, serv_port, strerror(errno));
    }

    /* try to open socket for upstream traffic */
    for (q=result; q!=NULL; q=q->ai_next) {
        sock_up = socket(q->ai_family, q->ai_socktype,q->ai_protocol);
        if (sock_up == -1) continue; /* try next field */
        else break; /* success, get out of loop */
    }

    /* connect so we can send/receive packet with the server only */
    ret = connect(sock_up, q->ai_addr, q->ai_addrlen);
    if (ret != 0) {
        CON_PRINTF("ERROR: [up] connect returned %s\n", strerror(errno));
        printf("socket_up initialization failed...\n");
        return -1;
    }

    /* set upstream socket RX timeout */
    ret = setsockopt(sock_up, SOL_SOCKET, SO_RCVTIMEO, (void *)&push_timeout_half, sizeof push_timeout_half);
    if (ret != 0) {
        MSG("ERROR: [up] setsockopt returned %s\n", strerror(errno));
        return -1;
    }

    printf("socket_up initialized...\n");
    freeaddrinfo(result);

    //外网 npwl.npiot.com:44220
    /* look for server address w/ downstream port */
    ret = getaddrinfo(serv_addr, serv_port, &hints, &result);
    if (ret != 0) {
        CON_PRINTF("ERROR: [up] getaddrinfo on address %s (PORT %s) returned %s\n", serv_addr, serv_port, strerror(errno));
    }

    /* try to open socket for upstream traffic */
    for (q=result; q!=NULL; q=q->ai_next) {
        sock_down = socket(q->ai_family, q->ai_socktype,q->ai_protocol);
        if (sock_down == -1) continue; /* try next field */
        else break; /* success, get out of loop */
    }

    /* connect so we can send/receive packet with the server only */
    ret = connect(sock_down, q->ai_addr, q->ai_addrlen);
    if (ret != 0)
    {
        CON_PRINTF("ERROR: [up] connect returned %s\n", strerror(errno));
        printf("socket_down initialization failed...\n");
        return -1;
    }

    ret = setsockopt(sock_down, SOL_SOCKET, SO_RCVTIMEO, (void*)&pull_timeout, sizeof(pull_timeout));
    if(ret < 0 )
    {
        ESP_LOGE("SOCKET-INIT", "Error occured during setsockopt: errno %d", errno);
        return -1;
    }

    printf("socket_down initialized...\n");
    freeaddrinfo(result);

    ESP_LOGI("GEN FD","uplink_fd:%d downlink_fd:%d",sock_up,sock_down);

    return 0;
}

int lora_pkt_fwd_init(char* server_ip,char* server_port)
{

    jit_protect_semaphore = xSemaphoreCreateMutex();  
    if(jit_protect_semaphore == NULL)
    {
        printf("jit_protect_semaphore xSemaphoreCreateMutex failed \n");  
    }
    else 
    {
        xSemaphoreGive(jit_protect_semaphore);
    }
    /*************************LORA DRIVER INIT END**********************************/

    if(generate_pkt_fwd_fd(server_ip,server_port))
    {
        pkt_fwd_runningException_handler();
    }

    //硬件初始化成功，开启LoRaWAN模式，否则不开启
    xTaskCreate(&thread_up, "thread_up", 1024*6, NULL, 7, NULL);

    xTaskCreate(&thread_down, "thread_down", 1024*6, NULL, 7, NULL);

    xTaskCreate(&thread_jit, "thread_jit", 1024*6, NULL, 6, NULL);

    xTaskCreate(&thread_stat, "thread_stat", 4096, NULL, 6, NULL);

    return 0;

}


void thread_up(void* args)
{
    printf("thread_up start up...\n");    
    RF_Parameters_t * p_pkt_rx[10];
    RF_Parameters_t* p = NULL;

    uint8_t buff_up[1024];
    int buff_index = 0;

    int nb_pkt = 0;
    int i = 0;

    int j; /* loop variables */
    unsigned pkt_in_dgram; /* nb on Lora packet in the current datagram */

    uint8_t buff_ack[32]; /* buffer to receive acknowledges */

    /* protocol variables */
    uint8_t token_h; /* random token for acknowledgement matching */
    uint8_t token_l; /* random token for acknowledgement matching */

    /* report management variable */
    bool send_report = false;

    /* mote info variables */
    uint32_t mote_addr = 0;
    uint16_t mote_fcnt = 0;

    /* pre-fill the data buffer with fixed fields */
    buff_up[0] = PROTOCOL_VERSION;
    buff_up[3] = PKT_PUSH_DATA;
    *(uint32_t *)(buff_up + 4) = net_mac_h;
    *(uint32_t *)(buff_up + 8) = net_mac_l;

    while(1)
    {
        nb_pkt = 0;
        while(xQueueReceive(radioRxdone_evt_queue, &p_pkt_rx[nb_pkt], 5))
        {
            nb_pkt++;
        }

        /* check if there are status report to send */
        send_report = report_ready; /* copy the variable so it doesn't change mid-function */
        /* no mutex, we're only reading */

        /* wait a short time if no packets, nor status report */
        if ((nb_pkt == 0) && (send_report == false)) {
            // vTaskDelay(10);
            continue;
        }

        /* start composing datagram with the header */
        token_h = (uint8_t)rand(); /* random token */
        token_l = (uint8_t)rand(); /* random token */
        buff_up[1] = token_h;
        buff_up[2] = token_l;
        buff_index = 12; /* 12-byte header */

        /* start of JSON structure */
        memcpy((void *)(buff_up + buff_index), (void *)"{\"rxpk\":[", 9);
        buff_index += 9;

        /* serialize Lora packets metadata and payload */
        pkt_in_dgram = 0;
        for (i=0; i < nb_pkt; ++i) 
        {
            p = p_pkt_rx[i];
            /* Get mote information from current packet (addr, fcnt) */
            /* FHDR - DevAddr */
            mote_addr  = p->payload[1];
            mote_addr |= p->payload[2] << 8;
            mote_addr |= p->payload[3] << 16;
            mote_addr |= p->payload[4] << 24;
            /* FHDR - FCnt */
            mote_fcnt  = p->payload[6];
            mote_fcnt |= p->payload[7] << 8;

            /* basic packet filtering */
            pthread_mutex_lock(&mx_meas_up);
            meas_nb_rx_rcv += 1;
            switch(p->rx_info.status) {
                case STAT_CRC_OK:
                    meas_nb_rx_ok += 1;
                    CON_PRINTF( "\nINFO: Received pkt from mote: %08X (fcnt=%u)\n", mote_addr, mote_fcnt );
                    break;
                case STAT_CRC_BAD:
                    meas_nb_rx_bad += 1;
                    break;
                case STAT_NO_CRC:
                    meas_nb_rx_nocrc += 1;
                    break;
                default:
                    CON_PRINTF("WARNING: [up] received packet with unknown status \n");
                    pthread_mutex_unlock(&mx_meas_up);
                    continue; /* skip that packet */
                    // exit(EXIT_FAILURE);
            }
            meas_up_pkt_fwd += 1;
            meas_up_payload_byte += p->size;
            pthread_mutex_unlock(&mx_meas_up);    

            /* Start of packet, add inter-packet separator if necessary */
            if (pkt_in_dgram == 0) {
                buff_up[buff_index] = '{';
                ++buff_index;
            } else {
                buff_up[buff_index] = ',';
                buff_up[buff_index+1] = '{';
                buff_index += 2;
            }

            uint64_t count_us = p->rx_info.count_ms*1000;
            /* RAW timestamp, 8-17 useful chars */
            j = snprintf((char *)(buff_up + buff_index), TX_BUFF_SIZE-buff_index, "\"tmst\":%llu", count_us);
            if (j > 0) 
            {
                buff_index += j;
            } 
            else 
            {
                CON_PRINTF("ERROR: [up] snprintf failed line %u\n", (__LINE__ - 4));
                continue;
            }

            /* Packet concentrator channel, RF chain & RX frequency, 34-36 useful chars */
            j = snprintf((char *)(buff_up + buff_index), TX_BUFF_SIZE-buff_index, ",\"chan\":%1u,\"rfch\":%1u,\"freq\":%.6lf", p->rx_info.if_chain, p->rx_info.rf_chain, ((double)p->freq_hz / 1e6));
            if (j > 0) {
                buff_index += j;
            } else {
                CON_PRINTF("ERROR: [up] snprintf failed line %u\n", (__LINE__ - 4));
                continue;
            }

            /* Packet status, 9-10 useful chars */
            switch (p->rx_info.status) {
                case STAT_CRC_OK:
                    memcpy((void *)(buff_up + buff_index), (void *)",\"stat\":1", 9);
                    buff_index += 9;
                    break;
                case STAT_CRC_BAD:
                    memcpy((void *)(buff_up + buff_index), (void *)",\"stat\":-1", 10);
                    buff_index += 10;
                    break;
                case STAT_NO_CRC:
                    memcpy((void *)(buff_up + buff_index), (void *)",\"stat\":0", 9);
                    buff_index += 9;
                    break;
                default:
                    CON_PRINTF("ERROR: [up] received packet with unknown status\n");
                    memcpy((void *)(buff_up + buff_index), (void *)",\"stat\":?", 9);
                    buff_index += 9;
                    continue;
            }

            /* Packet modulation, 13-14 useful chars */
            if (p->modulation == MODULATION_LORA) {
                memcpy((void *)(buff_up + buff_index), (void *)",\"modu\":\"LORA\"", 14);
                buff_index += 14;

                /* Lora datarate & bandwidth, 16-19 useful chars */
                switch (p->datarate) {
                    case LoRa_SF7:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"datr\":\"SF7", 12);
                        buff_index += 12;
                        break;
                    case LoRa_SF8:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"datr\":\"SF8", 12);
                        buff_index += 12;
                        break;
                    case LoRa_SF9:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"datr\":\"SF9", 12);
                        buff_index += 12;
                        break;
                    case LoRa_SF10:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"datr\":\"SF10", 13);
                        buff_index += 13;
                        break;
                    case LoRa_SF11:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"datr\":\"SF11", 13);
                        buff_index += 13;
                        break;
                    case LoRa_SF12:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"datr\":\"SF12", 13);
                        buff_index += 13;
                        break;
                    default:
                        CON_PRINTF("ERROR: [up] lora packet with unknown datarate\n");
                        memcpy((void *)(buff_up + buff_index), (void *)",\"datr\":\"SF?", 12);
                        buff_index += 12;
                        continue;
                }
                switch (p->bandwidth) {
                    case BANDWIDTH_125KHZ:
                        memcpy((void *)(buff_up + buff_index), (void *)"BW125\"", 6);
                        buff_index += 6;
                        break;
                    case BANDWIDTH_250KHZ:
                        memcpy((void *)(buff_up + buff_index), (void *)"BW250\"", 6);
                        buff_index += 6;
                        break;
                    case BANDWIDTH_500KHZ:
                        memcpy((void *)(buff_up + buff_index), (void *)"BW500\"", 6);
                        buff_index += 6;
                        break;
                    default:
                        CON_PRINTF("ERROR: [up] lora packet with unknown bandwidth\n");
                        memcpy((void *)(buff_up + buff_index), (void *)"BW?\"", 4);
                        buff_index += 4;
                        continue;
                }

                /* Packet ECC coding rate, 11-13 useful chars */
                switch (p->coderate) {
                    case LORA_CR_4_5:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"codr\":\"4/5\"", 13);
                        buff_index += 13;
                        break;
                    case LORA_CR_4_6:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"codr\":\"4/6\"", 13);
                        buff_index += 13;
                        break;
                    case LORA_CR_4_7:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"codr\":\"4/7\"", 13);
                        buff_index += 13;
                        break;
                    case LORA_CR_4_8:
                        memcpy((void *)(buff_up + buff_index), (void *)",\"codr\":\"4/8\"", 13);
                        buff_index += 13;
                        break;
                    default:
                        CON_PRINTF("ERROR: [up] lora packet with unknown coderate\n");
                        memcpy((void *)(buff_up + buff_index), (void *)",\"codr\":\"?\"", 11);
                        buff_index += 11;
                        continue;
                }

                /* Lora SNR, 11-13 useful chars */
                j = snprintf((char *)(buff_up + buff_index), TX_BUFF_SIZE-buff_index, ",\"lsnr\":%.1f", p->rx_info.lsnr);
                if (j > 0) {
                    buff_index += j;
                } else {
                    CON_PRINTF("ERROR: [up] snprintf failed line %u\n", (__LINE__ - 4));
                    continue;
                }
            } else if (p->modulation == MODULATION_FSK) {
                memcpy((void *)(buff_up + buff_index), (void *)",\"modu\":\"FSK\"", 13);
                buff_index += 13;

                /* FSK datarate, 11-14 useful chars */
                j = snprintf((char *)(buff_up + buff_index), TX_BUFF_SIZE-buff_index, ",\"datr\":%u", p->datarate);
                if (j > 0) {
                    buff_index += j;
                } else {
                    CON_PRINTF("ERROR: [up] snprintf failed line %u\n", (__LINE__ - 4));
                    continue;
                }
            } else {
                CON_PRINTF("ERROR: [up] received packet with unknown modulation\n");
                continue;
            }

            /* Packet RSSI, payload size, 18-23 useful chars */
            j = snprintf((char *)(buff_up + buff_index), TX_BUFF_SIZE-buff_index, ",\"rssi\":%.0f,\"size\":%u", p->rx_info.rssi, p->size);
            if (j > 0) {
                buff_index += j;
            } else {
                CON_PRINTF("ERROR: [up] snprintf failed line %u\n", (__LINE__ - 4));
                continue;
            }

            /* Packet base64-encoded payload, 14-350 useful chars */
            memcpy((void *)(buff_up + buff_index), (void *)",\"data\":\"", 9);
            buff_index += 9;
            j = bin_to_b64(p->payload, p->size, (char *)(buff_up + buff_index), 341); /* 255 bytes = 340 chars in b64 + null char */
            if (j>=0) {
                buff_index += j;
            } else {
                CON_PRINTF("ERROR: [up] bin_to_b64 failed line %u\n", (__LINE__ - 5));
                continue;
            }
            buff_up[buff_index] = '"';
            ++buff_index;

            /* End of packet serialization */
            buff_up[buff_index] = '}';
            ++buff_index;
            ++pkt_in_dgram; 

            free(p_pkt_rx[i]);   
        }

        /* restart fetch sequence without sending empty JSON if all packets have been filtered out */
        if (pkt_in_dgram == 0) {
            if (send_report == true) {
                /* need to clean up the beginning of the payload */
                buff_index -= 8; /* removes "rxpk":[ */
            } else {
                /* all packet have been filtered out and no report, restart loop */
                continue;
            }
        } else {
            /* end of packet array */
            buff_up[buff_index] = ']';
            ++buff_index;
            /* add separator if needed */
            if (send_report == true) {
                buff_up[buff_index] = ',';
                ++buff_index;
            }
        }

        /* add status report if a new one is available */
        if (send_report == true) {
            pthread_mutex_lock(&mx_stat_rep);
            report_ready = false;
            j = snprintf((char *)(buff_up + buff_index), TX_BUFF_SIZE-buff_index, "%s", status_report);
            pthread_mutex_unlock(&mx_stat_rep);
            if (j > 0) {
                buff_index += j;
            } else {
                CON_PRINTF("ERROR: [up] snprintf failed line %u\n", (__LINE__ - 5));
                continue;
            }
        }    

        /* end of JSON datagram payload */
        buff_up[buff_index] = '}';
        ++buff_index;
        buff_up[buff_index] = 0; /* add string terminator, for safety */

        CON_PRINTF("\n[ %ld ]JSON up[%d]: %s\n",get_GWMPTick(),strlen((char *)(buff_up + 12)), (char *)(buff_up + 12)); /* DEBUG: display JSON payload */
        ESP_LOGI("thread up","msg_len:%d",buff_index);
        
        if(udp_write(sock_up, buff_up, buff_index, (char*)__FUNCTION__))
        {
            close(sock_up);
            pkt_fwd_runningException_handler();
        }


        /* wait for acknowledge (in 2 times, to catch extra packets) */
        for (i=0; i<2; ++i) {
            j = recv(sock_up, (void *)buff_ack, sizeof buff_ack, 0);
            if (j == -1) {
                if (errno == EAGAIN) { /* timeout */
                    continue;
                } else { /* server connection error */
                    break;
                }
            } else if ((j < 4) || (buff_ack[0] != PROTOCOL_VERSION) || (buff_ack[3] != PKT_PUSH_ACK)) {
                MSG("WARNING: [up] ignored invalid non-ACL packet\n");
                continue;
            } else if ((buff_ack[1] != token_h) || (buff_ack[2] != token_l)) {
                MSG("WARNING: [up] ignored out-of sync ACK packet\n");
                continue;
            } else {
                MSG("INFO: [up] ACK packet RECEVIED\n");
                meas_up_ack_rcv += 1;
                break;
            }
        }
        // pthread_mutex_unlock(&mx_meas_up);
    }

    vTaskDelete(NULL);
}


void thread_down(void* args)
{
    printf("thread_down start up...\n");
    // bool sent_immediate = false; /* option to sent the packet immediately */

    /* local timekeeping variables */
    uint32_t send_time = 0; /* time of the pull request */
    // uint32_t recv_time; /* time of return from recv socket call */

    /* data buffers */
    uint8_t buff_down[1000]; /* buffer to receive downstream packets */
    uint8_t buff_req[12]; /* buffer to compose pull requests */
    uint8_t buff_ack[64];
    int msg_len = 0;

    // bool req_ack = false; /* keep track of whether PULL_DATA was acknowledged or not */

    /* Just In Time downlink */
    uint32_t current_concentrator_time;
    enum jit_error_e jit_result = JIT_ERROR_OK;
    enum jit_pkt_type_e downlink_type;

    uint32_t keepalive_time = 10000; /* send a PULL_DATA request every X seconds, negative = disabled */


    /* JIT queue initialization */
    jit_queue_init(&jit_queue);
    while (1)
    {
        get_PULL_DATA_frame(buff_req);

        /* send PULL request and record time */
        if(udp_write(sock_down, buff_req, sizeof buff_req, (char*)__FUNCTION__))
        {
            close(sock_down);
            pkt_fwd_runningException_handler();
        }
        send_time = xTaskGetTickCount();

        CON_PRINTF_ARRAY((char*)buff_req,sizeof(buff_req));
        // req_ack = false;

        // /* listen to packets and process them until a new PULL request must be sent */
        // recv_time = send_time;
        while ((xTaskGetTickCount() - send_time) < keepalive_time)
        {
            /* try to receive a datagram */
            msg_len = recv(sock_down, (void *)buff_down, (sizeof buff_down)-1, 0);

            /* if no network message was received, got back to listening sock_down socket */
            if (msg_len == -1)
            {
                continue;
            }
            else if(msg_len == 0)
            {
                perror("thread down");
            }
            
            CON_PRINTF("\n[ %ld ] Recv MSG Down,msg_len:%d\n",get_GWMPTick(),msg_len); /* DEBUG: display JSON payload */

            /* if the datagram does not respect protocol, just ignore it */
            if ((msg_len < 4) || (buff_down[0] != PROTOCOL_VERSION) || ((buff_down[3] != PKT_PULL_RESP) && (buff_down[3] != PKT_PULL_ACK))) {
                CON_PRINTF("WARNING: [down] ignoring invalid packet len=%d, protocol_version=%d, id=%d\n",
                        msg_len, buff_down[0], buff_down[3]);
                continue;
            }

            /* if the datagram is an ACK, check token */
            if (buff_down[3] == PKT_PULL_ACK)
            {
                continue;
            }

            //只有PULL_RESP的数据才会被处理
            /* the datagram is a PULL_RESP */
            buff_down[msg_len] = 0; /* add string terminator, just to be safe */
            CON_PRINTF("INFO: [down] PULL_RESP received  - token[%d:%d] :)\n", buff_down[1], buff_down[2]); /* very verbose */
            CON_PRINTF("\n[ %ld ]JSON down[%d]: %s\n",get_GWMPTick(), strlen((char *)(buff_down + 4)),(char *)(buff_down + 4)); /* DEBUG: display JSON payload */

            /* configuration and metadata for an outbound packet */
            RF_Parameters_t* p_txpkt = (RF_Parameters_t*)malloc(sizeof(RF_Parameters_t));

            /* initialize TX struct and try to parse JSON */
            if(txpkjson_to_rfPara(p_txpkt,buff_down + 4))
            {
                CON_PRINTF("txpk Error\n");
                continue;
            }
            else
            {
                CON_PRINTF("parse txpk SUCCESS\n");
            }

            if(p_txpkt->tx_info.imme == 1)
            {
                p_txpkt->tx_info.tx_mode = IMMEDIATE;

                downlink_type = JIT_PKT_TYPE_DOWNLINK_CLASS_C;

                CON_PRINTF("class C Downlink frame\n");
            }
            else
            {
                p_txpkt->tx_info.tx_mode = TIMESTAMPED;

                downlink_type = JIT_PKT_TYPE_DOWNLINK_CLASS_A;
            
                CON_PRINTF("class A Downlink frame\n");
            }

            /* record measurement data */
            pthread_mutex_lock(&mx_meas_dw);
            meas_dw_dgram_rcv += 1; /* count only datagrams with no JSON errors */
            meas_dw_network_byte += msg_len; /* meas_dw_network_byte */
            meas_dw_payload_byte += p_txpkt->size;
            pthread_mutex_unlock(&mx_meas_dw);


            /* check TX parameter before trying to queue packet */
            jit_result = JIT_ERROR_OK;
            /* insert packet to be sent into JIT queue */
            if (jit_result == JIT_ERROR_OK)
            {
                current_concentrator_time = get_GWMPTick();
                p_txpkt->tx_info.count_ms += 5;//往后延5ms,尽量保证节点窗口期打开了再发下行包
                if( xSemaphoreTake( jit_protect_semaphore, 30  ) != pdTRUE )
                {
                    CON_PRINTF("%s xSemaphoreTake take fail\n",__func__);
                    continue;
                }
                jit_result = jit_enqueue(&jit_queue, current_concentrator_time, p_txpkt, downlink_type);
                free(p_txpkt);
                xSemaphoreGive(jit_protect_semaphore);
                if (jit_result != JIT_ERROR_OK) {
                    CON_PRINTF("ERROR: Packet REJECTED (jit error=%d)\n", jit_result);
                }
            }

            int buff_ack_len = get_tx_ack(buff_down[1], buff_down[2], jit_result,buff_ack); 
            if(udp_write(sock_down, buff_ack, buff_ack_len, (char*)__FUNCTION__))
            {
                close(sock_down);
                pkt_fwd_runningException_handler();
            }

            CON_PRINTF("tx_ack(%d):%s\n",buff_ack_len,buff_ack+12);

            // /* Send acknoledge datagram to server */
            // send_tx_ack(buff_down[1], buff_down[2], jit_result);
        }
    }
    CON_PRINTF("\nINFO: End of downstream thread\n");

    vTaskDelete(NULL);
}

void thread_jit(void* args)
{
    printf("thread_jit start up...\n");
    int result = 0;
    RF_Parameters_t pkt;
    int pkt_index = -1;
    uint32_t current_concentrator_time;
    enum jit_error_e jit_result;
    enum jit_pkt_type_e pkt_type;
//    uint8_t tx_status;

    while (1)
    {
        vTaskDelay(3);

        current_concentrator_time = get_GWMPTick();
        if( xSemaphoreTake( jit_protect_semaphore, 30  ) != pdTRUE )
        {
            CON_PRINTF("%s xSemaphoreTake take fail\n",__func__);
            continue;
        }
        jit_result = jit_peek(&jit_queue, current_concentrator_time, &pkt_index);
        xSemaphoreGive(jit_protect_semaphore);
       
        if (jit_result == JIT_ERROR_OK)
        {
            if (pkt_index > -1)
            {
                if( xSemaphoreTake( jit_protect_semaphore, 30  ) != pdTRUE )
                {
                    // CON_PRINTF("%s xSemaphoreTake take fail\n",__func__);
                    continue;
                }
                jit_result = jit_dequeue(&jit_queue, pkt_index, &pkt, &pkt_type);
                xSemaphoreGive(jit_protect_semaphore);

                if (jit_result == JIT_ERROR_OK)
                {
                    /* update beacon stats */
                    if (pkt_type == JIT_PKT_TYPE_BEACON)
                    {
                        CON_PRINTF("INFO: Beacon dequeued (count_ms=%u)\n", pkt.tx_info.count_ms);
                    }

                    result = interface_LoRaPhy_status();
                    if(result == PHY_STATUS_TX)//如果设备正在发送，则不进行下发数据
                    {
                        ESP_LOGI("thread JIT","sending,waiting for next time");
                        continue;
                    }
                    ESP_LOGI("thread JIT","RF is not TX Running");

                    /* send packet to concentrator */
                    result = interface_LoRaPhy_SendFrame(&pkt);
                    CON_PRINTF("\n[ %ld ] Send Frame\n",get_GWMPTick());

                    pthread_mutex_lock(&mx_meas_dw);
                    meas_nb_tx_ok += 1;
                    pthread_mutex_unlock(&mx_meas_dw);

                    CON_PRINTF("downlink frame sent\n");
                } else {
                    CON_PRINTF("ERROR: jit_dequeue failed with %d\n", jit_result);
                }
            }
        } else if (jit_result == JIT_ERROR_EMPTY) {
            /* Do nothing, it can happen */
        } else {
            CON_PRINTF("ERROR: jit_peek failed with %d\n", jit_result);
        }
    }
    vTaskDelete(NULL);
}

void thread_stat(void* args)
{
    printf("thread_stat start up...\n");

    /* variables to get local copies of measurements */
    uint32_t cp_nb_rx_rcv;
    uint32_t cp_nb_rx_ok;
    uint32_t cp_up_pkt_fwd;
    uint32_t cp_up_dgram_sent;
    uint32_t cp_up_ack_rcv;
    uint32_t cp_dw_dgram_rcv;
    uint32_t cp_nb_tx_ok;
    uint32_t cp_nb_tx_requested = 0;

    /* statistics variable */
    time_t t;
    char stat_timestamp[24];
    float up_ack_ratio;

    uint32_t last_report_time = 0;
    while(1)
    {
        if(xTaskGetTickCount()-last_report_time > 1000*30)
        {
            /* get timestamp for statistics */
            t = time(NULL);
            strftime(stat_timestamp, sizeof stat_timestamp, "%F %T %Z", gmtime(&t));

            /* access upstream statistics, copy and reset them */
            pthread_mutex_lock(&mx_meas_up);
            cp_nb_rx_rcv       = meas_nb_rx_rcv;
            cp_nb_rx_ok        = meas_nb_rx_ok;
            cp_up_pkt_fwd      = meas_up_pkt_fwd;
            cp_up_dgram_sent   = meas_up_dgram_sent;
            cp_up_ack_rcv      = meas_up_ack_rcv;
            meas_nb_rx_rcv = 0;
            meas_nb_rx_ok = 0;
            meas_nb_rx_bad = 0;
            meas_up_pkt_fwd = 0;
            meas_up_network_byte = 0;
            meas_up_payload_byte = 0;
            meas_up_ack_rcv = 0;
            pthread_mutex_unlock(&mx_meas_up);
            if (cp_up_dgram_sent > 0) {
                up_ack_ratio = (float)cp_up_ack_rcv / (float)cp_up_dgram_sent;
            } else {
                up_ack_ratio = 0.0;
            }

            /* access downstream statistics, copy and reset them */
            pthread_mutex_lock(&mx_meas_dw);
            cp_dw_dgram_rcv    =  meas_dw_dgram_rcv;
            cp_nb_tx_ok        =  meas_nb_tx_ok;
            cp_nb_tx_requested                 +=  meas_nb_tx_requested;
            meas_dw_pull_sent = 0;
            meas_dw_ack_rcv = 0;
            meas_dw_dgram_rcv = 0;
            meas_dw_network_byte = 0;
            meas_dw_payload_byte = 0;
            meas_nb_tx_ok = 0;
            meas_nb_tx_fail = 0;
            meas_nb_tx_requested = 0;
            meas_nb_tx_rejected_collision_packet = 0;
            meas_nb_tx_rejected_collision_beacon = 0;
            meas_nb_tx_rejected_too_late = 0;
            meas_nb_tx_rejected_too_early = 0;
            meas_nb_beacon_queued = 0;
            meas_nb_beacon_sent = 0;
            meas_nb_beacon_rejected = 0;
            pthread_mutex_unlock(&mx_meas_dw);

            /* generate a JSON report (will be sent to server by upstream thread) */
            pthread_mutex_lock(&mx_stat_rep);
            snprintf(status_report, STATUS_SIZE, "\"stat\":{\"time\":\"%s\",\"rxnb\":%u,\"rxok\":%u,\"rxfw\":%u,\"ackr\":%.1f,\"dwnb\":%u,\"txnb\":%u}", stat_timestamp, cp_nb_rx_rcv, cp_nb_rx_ok, cp_up_pkt_fwd, 100.0 * up_ack_ratio, cp_dw_dgram_rcv, cp_nb_tx_ok);
            report_ready = true;
            pthread_mutex_unlock(&mx_stat_rep);

            last_report_time = xTaskGetTickCount();
        }

        vTaskDelay(1000);
    }



    vTaskDelete(NULL);
}

static int udp_write(int fd,uint8_t* buff,int len,char* func_name)
{
    if(send(fd, buff, len, 0) != len)
    {
        perror(func_name);

        return -1;
    }

    return 0;
}
